#define _USE_MATH_DEFINES
#include <cmath>
#include "AlgoEvaluatorModule6NavigationDistance.h"

class algo::AlgoEvaluatorModule6NavigationDistancePrivate 
{
public:
};

algo::AlgoEvaluatorModule6NavigationDistance::AlgoEvaluatorModule6NavigationDistance()
	: d(new AlgoEvaluatorModule6NavigationDistancePrivate)
{
}

algo::AlgoEvaluatorModule6NavigationDistance::~AlgoEvaluatorModule6NavigationDistance()
{
	delete d;
}

std::vector<algobase::EvaluateEnvirDataType> algo::AlgoEvaluatorModule6NavigationDistance::getNeedType()
{
	return std::vector<algobase::EvaluateEnvirDataType>();
}

// Helper function to calculate distance between two lat/lon points in meters using Haversine formula.
static double haversine_distance(double lat1, double lon1, double lat2, double lon2) {
    const double R = 6371000.0; // Earth radius in meters
    auto to_radians = [](double degree) {
        return degree * M_PI / 180.0;
    };

    double lat1_rad = to_radians(lat1);
    double lon1_rad = to_radians(lon1);
    double lat2_rad = to_radians(lat2);
    double lon2_rad = to_radians(lon2);

    double dlat = lat2_rad - lat1_rad;
    double dlon = lon2_rad - lon1_rad;

    double a = sin(dlat / 2.0) * sin(dlat / 2.0) +
               cos(lat1_rad) * cos(lat2_rad) *
               sin(dlon / 2.0) * sin(dlon / 2.0);
    double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));

    return R * c;
}

int algo::AlgoEvaluatorModule6NavigationDistance::exec(const algobase::Path& path, double& result, algobase::EvaluateModuleCoords& indexCoords)
{
	if (path.size() < 2) {
		result = 0.0;
		indexCoords.clear();
		return 0;
	}

	double total_distance_meters = 0.0;
	indexCoords.resize(path.size());
	indexCoords[0].Assign(path[0]);
	indexCoords[0].mRawValue = 0;
	indexCoords[0].mResult = 0.0; // Distance at the start is 0.

	for (size_t i = 0; i < path.size() - 1; ++i) {
		const auto& p1 = path[i];
		const auto& p2 = path[i+1];

		// Horizontal distance
		double h_dist = haversine_distance(p1.mCoordZ[1], p1.mCoordZ[0], p2.mCoordZ[1], p2.mCoordZ[0]);

		// Vertical distance (assuming Z is in meters)
		double v_dist = std::abs(p2.mCoordZ[2] - p1.mCoordZ[2]);

		// 3D segment distance
		double segment_dist = std::sqrt(h_dist * h_dist + v_dist * v_dist);
		total_distance_meters += segment_dist;

		// Store the INSTANTANEOUS segment distance at the start point of the segment.
		indexCoords[i].Assign(p1);
		indexCoords[i].mRawValue = segment_dist;
		indexCoords[i].mResult = calcScore(segment_dist);
	}
	// Last point has no outgoing segment.
	indexCoords.back().Assign(path.back());
	indexCoords.back().mResult = 0.0;

	// The final raw value is the total distance in kilometers.
	result = calcScore(total_distance_meters / 1000.0);

	return 0;
}

double algo::AlgoEvaluatorModule6NavigationDistance::calcScore(double rawValue) const
{
	double refDistance_m = 5000.0; // Typical segment length in meters
	return costSigmoid(rawValue, refDistance_m, refDistance_m * 0.2) * getWeight();
}

double algo::AlgoEvaluatorModule6NavigationDistance::calcRaw(const std::vector<double>& rawValues) const
{
	double total_dist = 0;
	for (double val : rawValues) {
		total_dist += val;
	}
	return total_dist / 1000.0;
}
